#include <robot/kuka/cvirtualarm.h>

#include <buola/io/curi.h>
#include <buola/scene/cgeode.h>

#define ROBOT_SHARE_DIR "/usr/local/share/robot"

namespace buola { namespace robot { namespace kuka {

CVirtualArm::CVirtualArm()
{
    mGroup=new scene::CRTTransform;
//    mGroup->SetRotation(mat::CQuat_d(M_PI_2,mat::CVec3d(0,0,1)));
    mGroup->SetTranslation(mat::CVec3d(0,0,0.185));
    
    scene::PGeode lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/KUKA/kuka_0.iv",0.001);
    lGeode->AttachTo(mGroup);

    mJoints[0]=new scene::CRevolutionJoint({0,0,0},{0,0,-1});
    mGroup->AddChild(mJoints[0]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/KUKA/kuka_1.iv",0.001);
    lGeode->AttachTo(mJoints[0]);

    mJoints[1]=new scene::CRevolutionJoint({0.075,0.0545,0.150},{0,1,0},M_PI_2);
    mJoints[0]->AddChild(mJoints[1]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/KUKA/kuka_2.iv",0.001);
    lGeode->AttachTo(mJoints[1]);

    mJoints[2]=new scene::CRevolutionJoint({0.0,-0.0055,0.365},{0,1,0},-M_PI_2);
    mJoints[1]->AddChild(mJoints[2]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/KUKA/kuka_3.iv",0.001);
    lGeode->AttachTo(mJoints[2]);

    mJoints[3]=new scene::CRevolutionJoint({0.198,-0.057,0.090},{-1,0,0});
    mJoints[2]->AddChild(mJoints[3]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/KUKA/kuka_4.iv",0.001);
    lGeode->AttachTo(mJoints[3]);

    mJoints[4]=new scene::CRevolutionJoint({0.207,-0.019,0},{0,1,0});
    mJoints[3]->AddChild(mJoints[4]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/KUKA/kuka_5.iv",0.001);
    lGeode->AttachTo(mJoints[4]);

    mJoints[5]=new scene::CRevolutionJoint({0.042,0.019,0},{-1,0,0});
    mJoints[4]->AddChild(mJoints[5]);
    lGeode=scene::CGeode::Import(io::file(ROBOT_SHARE_DIR)/"models/KUKA/kuka_6.iv",0.001);
    lGeode->AttachTo(mJoints[5]);

    mEndEffector=new scene::CRTTransform;
    mEndEffector->SetTranslation(mat::CVec3d(0.089,0,0));
    mEndEffector->SetRotation(mat::CQuat_d(M_PI_2,mat::CVec3d(0,0,1)));
    mJoints[5]->AddChild(mEndEffector);
}

CVirtualArm::~CVirtualArm()
{
}

void CVirtualArm::SetPose(const robot::kuka::CArmPose &pPose)
{
    lock_guard<mutex> lLock(mMutex);
    mPose=pPose;
}

robot::kuka::CArmPose CVirtualArm::GetPose()
{
    lock_guard<mutex> lLock(mMutex);
    return mPose;
}

void CVirtualArm::Update()
{
    lock_guard<mutex> lLock(mMutex);
    
    for(int i=0;i<6;i++)
        mJoints[i]->SetValue(mPose[i]);
}

/*namespace kuka*/ } /*namespace robot*/ } /*namespace buola*/ }
